#include "discrete_linear_system.h"
namespace auto_ros
{
namespace control
{
DiscreteLinearSys::DiscreteLinearSys(uint state_dim,
									 uint control_dim, uint distur_dim, uint out_dim)
	: LinearSys(state_dim, control_dim, distur_dim, out_dim)
{
}
bool DiscreteLinearSys::calc_steady_state_ctr(
	const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
	Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control)
{
	//ny =1 solve Ax = b
	if ((state_dim_ + out_dim_) != (state_dim_ + control_dim_))
	{
		std::cout << "calc_steady_state_ctr failed,(state_dim_ + out_dim_) != (state_dim_ + control_dim_)" << std::endl;
		return false;
	}
	if (y_controlled_ref.rows() != out_dim_)
	{
		std::cout << "calc_steady_state_ctr failed,y_controlled_ref.rows() ! = out_dim_" << std::endl;
		return false;
	}

	Eigen::MatrixXd expand_matrix = Eigen::MatrixXd::Zero(state_dim_ + out_dim_, state_dim_ + control_dim_);
	Eigen::VectorXd b_vector = Eigen::VectorXd::Zero(state_dim_ + out_dim_);

	expand_matrix.block(0, 0, state_dim_, state_dim_) = state_matrix_ - Eigen::MatrixXd::Identity(state_dim_, state_dim_);
	expand_matrix.block(0, state_dim_, state_dim_, control_dim_) = control_matrix_;
	expand_matrix.block(state_dim_, 0, out_dim_, state_dim_) = out_state_matrix_;
	/*std::cout << "expand matrix is:" << std::endl
			  << expand_matrix << std::endl;
	std::cout << "expand_matrix inv is:" << std::endl
			  << expand_matrix.inverse() << std::endl;*/
	b_vector.block(0, 0, state_dim_, 1) = -distur_matrix_ * distur_ref;
	/*std::cout << "-distur_matrix_ * distur_ref is:" << std::endl
			  << -distur_matrix_ * distur_ref << std::endl;*/

	if (out_distur_matrix_.rows() != 0)
	{
		b_vector.block(state_dim_, 0, out_dim_, 1) = y_controlled_ref - out_distur_matrix_ * distur_ref;
	}
	else
	{
		b_vector.block(state_dim_, 0, out_dim_, 1) = y_controlled_ref;
	}
	Eigen::VectorXd steady_state_ctr_vector = expand_matrix.inverse() * b_vector;
	steady_state = steady_state_ctr_vector.block(0, 0, state_dim_, 1);
	steady_control = steady_state_ctr_vector.block(state_dim_, 0, control_dim_, 1);
	return true;
};
} // namespace control
} // namespace auto_ros